/**
 * @file state.c
 * @author zybz
 * @brief
 * @version 0.1
 * @date 2022-03-25
 *
 * @copyright Copyright (c) 2022 北京邮电大学
 *
 */
#include "state.h"
#include "oslib.h"
#include "Dog.h"
#include "math.h"
#include "tim.h"

STATE state = state_recognize_ball; // 当前状态，全局变量
uint8_t pass_speed_bump = 0;
osTimerId_t timer_speed_bump;
static void timer_speed_bump_callback(void *argument)
{
	pass_speed_bump++;
};


int count_yellow = 0;

uint8_t climb_first_flag = 0;
float climb_time = 0;
float downstair_time = 0;
/***** 动作函数 *****/ // 对应狗的运动实现方式，action 函数中应重定义 狗运动的状态 DOG_MODE_RUN

void action_walk() //慢走步态
{
	if (Dog.ctrl_mode != DOG_MODE_WALK)
	{
		Dog.ctrl_mode = DOG_MODE_WALK;
	}
}

void action_upstair() //爬楼步态
{
	Dog_Gait_Turn();
	if (Dog.ctrl_mode != DOG_MODE_UPSTAIR)
	{
		Dog.ctrl_mode = DOG_MODE_UPSTAIR;
	}
}

void action_downstair() //下楼步态
{
    Dog_Gait_Turn();
    if (Dog.ctrl_mode != DOG_MODE_DOWNSTAIR)
    {
        Dog.ctrl_mode = DOG_MODE_DOWNSTAIR;
    }
}

void action_stop() //停止步态
{
	if (Dog.ctrl_mode != DOG_MODE_RESET)
	{
		Dog.ctrl_mode = DOG_MODE_RESET;
	}
}

void action_balance()
{
    if (Dog.ctrl_mode != DOG_MODE_BALANCE)
    {
        Dog.ctrl_mode = DOG_MODE_BALANCE;
    }
}

void action_open() //开箱动作
{
	__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, 1500); // 开门
	osDelay(2000);
	__HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_1, 200); // 关门
}


PID_s TurnPID = {2.3, 0.06, 0.0005, 0, 0, 7, 0, 1, 100};
void adjust_turn_PID(float turn_limit)
{
	static float turn_angle_list[5] = {0, 0, 0, 0, 0};
	static int turn_angle_list_flag = 0;
	float now_angle = 0;

	if (DIRECTION == 0)
	{
		return;
	}
	else if (DIRECTION == 1)
	{
		now_angle = (float)ANGLE;
	}
	else if (DIRECTION == 2)
	{
		now_angle = -(float)ANGLE;
	}
	float turn_angle = 0;
	turn_angle = -0.0009 * PID_Release(&TurnPID, 0, now_angle);
	if (turn_limit > 0.014)
	{
		turn_limit = 0.014;
	}
	LIMIT(turn_angle, turn_limit);
	turn_angle_list[turn_angle_list_flag] = turn_angle;
	turn_angle_list_flag++;
	turn_angle_list_flag %= 5;
	turn_angle = (turn_angle_list[0] + turn_angle_list[1] + turn_angle_list[2] + turn_angle_list[3] + turn_angle_list[4] + 2 * turn_angle) / 7;
	LIMIT(turn_angle, turn_limit);
	uprintf("now_angle:%.5f turn_angle: %.5f flag %d\n\r", now_angle, turn_angle, turn_angle_list_flag);
	delta_dh[0] = turn_angle;
	delta_dh[1] = -turn_angle;
	delta_dh[2] = -turn_angle;
	delta_dh[3] = turn_angle;
	delta_z = -0.03;
	delta_x = 0.007;
	// delta_x = -0.2 * turn_angle+0.004;
	// uprintf("delta_x:%f\r\n",delta_x);
	// LIMIT(delta_x, turn_limit*0.6);
	Dog_Gait_Turn();
	osDelay(187);
}

// 弯道转弯
void adjust_turn()
{
	static int flag_straight = 0;
	static int flag_left = 0;
	static int flag_right = 0;
	switch (DIRECTION)
	{
	case STRAIGHT:
		if (ANGLE > 9) // 角度小于10才应该转正
			return;
		if (flag_straight == 0)
		{
			// 回正一点
			if (flag_left == 1)
			{
				delta_dh[0] = 0.01;
				delta_dh[1] = -0.01;
				delta_dh[2] = -0.01;
				delta_dh[3] = 0.01;
				Dog_Gait_Turn();
				Dog.ctrl_mode = DOG_MODE_RESET;
				osDelay(187);
			}
			if (flag_right == 1)
			{
				delta_dh[0] = -0.01;
				delta_dh[1] = 0.01;
				delta_dh[2] = 0.01;
				delta_dh[3] = -0.01;
				Dog_Gait_Turn();
				Dog.ctrl_mode = DOG_MODE_RESET;
				osDelay(187);
			}
			// 回正
			delta_dh[0] = 0;
			delta_dh[1] = 0;
			delta_dh[2] = 0;
			delta_dh[3] = 0;
			Dog_Gait_Turn();
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(187);
			// GO
			GAIT_S = 0.08;
			GAIT_H = 0.016;
			GAIT_T = 0.75;
			delta_z = -0.03;
			delta_x = 0.01;
			Dog.ctrl_mode = DOG_MODE_WALK;
			// 标志位复位
			flag_straight = 1;
			flag_left = 0;
			flag_right = 0;
			osDelay(187);
		}
		break;
	case LEFT:
		if (flag_left == 0)
		{
			if (flag_right == 1) // 从右切到左
			{
				delta_dh[0] = -0.01;
				delta_dh[1] = 0.01;
				delta_dh[2] = 0.01;
				delta_dh[3] = -0.01;
				Dog_Gait_Turn();
				Dog.ctrl_mode = DOG_MODE_RESET;
				osDelay(187);
			}
			// 先弯一点
			delta_dh[0] = 0.01;
			delta_dh[1] = -0.01;
			delta_dh[2] = -0.01;
			delta_dh[3] = 0.01;
			Dog_Gait_Turn();
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(187);
			// 再弯一点
			delta_dh[0] = 0.02;
			delta_dh[1] = -0.02;
			delta_dh[2] = -0.02;
			delta_dh[3] = 0.02;
			Dog_Gait_Turn();
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(187);
			// GO
			GAIT_S = 0.04;
			delta_x = 0.01;
			GAIT_H = 0.016;
			GAIT_T = 0.75;
			delta_z = 0;
			Dog.ctrl_mode = DOG_MODE_WALK;
			// 标志位复位
			flag_straight = 0;
			flag_left = 1;
			flag_right = 0;
			osDelay(187);
		}
		break;
	case RIGHT:
		if (flag_right == 0)
		{
			if (flag_left == 1) // 从左向右切换
			{
				delta_dh[0] = 0.01;
				delta_dh[1] = -0.01;
				delta_dh[2] = -0.01;
				delta_dh[3] = 0.01;
				Dog_Gait_Turn();
				Dog.ctrl_mode = DOG_MODE_RESET;
				osDelay(187);
			}
			// 先弯一点
			delta_dh[0] = -0.01;
			delta_dh[1] = 0.01;
			delta_dh[2] = 0.01;
			delta_dh[3] = -0.01;
			Dog_Gait_Turn();
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(187);
			// 再弯一点
			delta_dh[0] = -0.02;
			delta_dh[1] = 0.02;
			delta_dh[2] = 0.02;
			delta_dh[3] = -0.02;
			Dog_Gait_Turn();
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(187);
			// GO
			GAIT_S = 0.04;
			delta_x = 0.01;
			GAIT_H = 0.016;
			GAIT_T = 0.75;
			delta_z = 0;
			Dog.ctrl_mode = DOG_MODE_WALK;
			// 标志位复位
			flag_straight = 0;
			flag_left = 0;
			flag_right = 1;
			osDelay(187);
		}
		break;
	}
}
/***** 状态切换 *****/
void state_trans(STATE st)
{
    if(st < 0 || st > 8)
        return;
	static int N = 10; // 向openmv发送信息次
    state = st;
    uint8_t data = '1' + st;
    for (int i = 0; i < N; i++)
    {
        HAL_UART_Transmit(&huart3, &data, 1, 0xFFFF);
    }
}

/***** 状态执行 *****/
void state_machine_exe() {
    static int ball_color = 0;
    static int change_benbi = 0;
    static int state_sync_flag = 0;
    if (HEADER1 != 100)
    {
        return;
    }
	// 赛道进程
	if (!state_sync_flag)
	{OSLIB_Uart_Printf(&huart3, "%d\r\n", state);}
	state_sync_flag = (state_sync_flag + 1) % 10 ;
	switch (state)
	{
	case state_1_begin:
		action_walk();
		break;
	case state_recognize_ball:
//        uprintf("now is state_recognize_ball\n\r");
		//停下
		if (BALL != 1) // 拿到球
		{
            Servo_Sync_Torque(1);
            action_stop();
		}
		else // 拿到球的路上
		{
            if(COLOR == RED || COLOR == PRUPLE)
            {
                ball_color = COLOR; // 存储球的颜色
                timer_speed_bump = osTimerNew(
                        timer_speed_bump_callback, osTimerOnce, (void *)0, NULL);

            }
            state_trans(state_2_user1);
            osDelay(1500); //装球延时
            osTimerStart(timer_speed_bump, 4500U);
		}
		break;

	case state_2_user1:
//        uprintf("now is state_2_user1\n\r");
		if (COLOR != PRUPLE) // 去往用户一的路上
		{
            action_walk();
		}
		else // 到达紫色临界处
		{
            osDelay(187);
            if (ball_color == PRUPLE) // 是该处送快递
            {
//                uprintf("pruple ball send success\n\r");
                action_stop();
                osDelay(500);
                action_open();
                adjust_turn_PID(0.025);
                ball_color = NONE;
            }
            else // 不是该处快递
            {
                osDelay(500);
                // adjust_turn();
                // adjust_turn();
                adjust_turn_PID(0.02);
            }
            HAL_TIM_Base_Start_IT(&htim7);
            state_trans(state_3_yellow_climb);
		}
		break;

	case state_3_yellow_climb:
//        uprintf("now is state_3_yellow_climb\n\r");
		if (COLOR != YELLOW) // 用户1转弯到上桥的阶段
        {
//            adjust_turn();
            action_walk();
        }
		else
            {
                osDelay(500);
                action_upstair();
                // TODO: 要保存一下状态 识别到一次 yellow 持续上楼梯到一段时间
                if(COLOR == BLUE)
                    state_trans(state_4_blue_bridge);
            }
		break;
    case state_4_blue_bridge:
        if (COLOR != MAGENTA)
        {
            action_balance();
            action_walk();
        }
        else
        {
            osDelay(500);
            state_trans(state_5_magenta_downstair);
            downstair_time = Dog_time.s;
        }
        break;

    case state_5_magenta_downstair:
        action_downstair();
        if(Dog_time.s - downstair_time > 50)
        {
            action_stop();
            osDelay(500);
            state_trans(state_6_black_obstacle);
        }
        break;

	case state_6_black_obstacle:    //TODO 暂未修改

		// uprintf("now is state_4_black_obstacle\n\r");
		get_tick(&Dog_time);

		if (climb_first_flag == 1)
		{
//			uprintf("climb_first_flag\n\r");
			climb_time = Dog_time.s;
		}
//		uprintf("delta time %f \n\r", Dog_time.s - climb_time);
		if (Dog_time.s - climb_time > 70)
		{
			delta_h = 0;
			delta_dh[0] = 0;
			delta_dh[1] = 0;
			delta_dh[2] = 0;
			delta_dh[3] = 0;

			Dog.legs[0].configuration = 1;
			Dog.legs[1].configuration = 1;
			Dog.legs[2].configuration = 2;
			Dog.legs[3].configuration = 2;
			Dog.ctrl_mode = DOG_MODE_RESET;
			osDelay(1500);
			//3100 ms
			for(uint8_t i = 0 ; i < (7000 / 50) ;i++)
			{
				if (DIRECTION ==  RIGHT)
				{
					Dog.v_x = -0.02;
				}
				else if (DIRECTION == LEFT)
				{
					Dog.v_x = 0.02;
				}
				else 
				{
					Dog.v_x = 0.0;
				}
				Dog.v_z = 0;
				delta_x = -0.010;
				GAIT_H = 0.02;
				GAIT_T = 1;
				GAIT_S = 0.018;
				delta_z = 0.02;
				osDelay(50);
			}
			// Dog.v_x = 0.02;
			
			// adjust_direction();
			// adjust_turn();
			

			// delta_z = 0;
			// GAIT_H = 0.01;
			// GAIT_T = 1.5;
			// GAIT_S = 0.02;
			// adjust_turn_PID(0.02);
			// action_slow();
			// osDelay(4500);
//			bridge_flag = 1;
//			bridge_time = Dog_time.s;
			state_trans(state_7_user2);
		}

		break;

	case state_7_user2:
		// uprintf("now is state_5_user2\n\r");
		if (COLOR != RED)
		{
            osDelay(500);
            action_walk();
		}
		else
		{
            osDelay(187);
            if (ball_color == RED) // 快递送达
            {
//                uprintf("brown ball send success\n\r");
                action_open();
                // adjust_turn();
                GAIT_H = 0.016;
                GAIT_T = 0.75;
                GAIT_S = 0.07;
                adjust_turn_PID(0.02);
                ball_color = BLACK;
            }
            else
            {
                osDelay(1000);
                // adjust_turn();
                GAIT_H = 0.016;
                GAIT_T = 0.75;
                GAIT_S = 0.06;
                adjust_turn_PID(0.02);
            }
		}
		break;

//	case state_6_grass:
//		// uprintf("now is state_6_grass\n\r");
//		if (COLOR == GREEN)
//		{
//			state_trans(state_2_user1);
//			///clear_turn();
//			osDelay(500);
//			///action_slow();
//			adjust_turn_PID(0.02);
//		}
//		else // 棕色到绿色之间的转弯
//		{
//			GAIT_H = 0.017;
//			GAIT_T = 0.83;
//			GAIT_S = 0.073;
//			adjust_turn_PID(0.018);
//		}
//		break;
//
//	case state_7_user3:
//		// uprintf("now is state_7_user3\n\r");
//		if (COLOR == PRUPLE)
//		{
//			state_trans(state_1_begin);
//			osDelay(187);
//			if (ball_color == PRUPLE)
//			{
//				uprintf("brown ball send success\n\r");
//				action_open();
//				// adjust_turn();
//				adjust_turn_PID(0.02);
//				ball_color = BLACK;
//			}
//			else
//			{
//				osDelay(1000);
//				adjust_turn_PID(0.02);
//			}
//		}
//		else // 草地到紫色之间的直道
//		{
//			GAIT_H = 0.016;
//			GAIT_T = 0.75;
//			GAIT_S = 0.06;
//			adjust_turn_PID(0.02);
//		}
//		break;

	default:
		break;
	}
}